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Abstract 


A  short  tutorial  on  Homogeneous  Transforms  is  presented  covering  the  triple 
interpretation  of  a  homogeneous  transform  as  an  operator,  a  coordinate  frame, 
and  a  coordinate  transform.  The  operator  /  transform  duality  is  derived  and  its 
use  in  the  Denavit  Hartenberg  convention  is  explained.  Forward,  inverse,  and 
differential  kinematics  are  derived  for  a  simple  manipulator  to  illustrate  con¬ 
cepts. 

A  standard  set  of  coordinate  frames  is  proposed  for  wheeled  mobile  robots.  It  is 
shown  that  the  RPY  transform  serves  the  same  purpose  as  the  DH  matrix  in  this 
case.  It  serves  to  interface  with  vehicle  position  estimation  systems  of  all  kinds, 
to  control  and  model  pan/tilt  mechanisms  and  stabilized  platforms,  and  to  model 
the  rigid  transforms  from  place  to  place  on  the  vehicle.  Forward  and  inverse 
kinematics  and  the  Euler  angle  rate  to  the  angular  velocity  transform  are  derived 
for  the  RPY  transform. 

Projective  kinematics  for  ideal  video  cameras  and  laser  rangefinders,  and  the 
imaging  Jacobian  relating  world  space  and  image  space  is  derived.  Finally,  the 
kinematics  of  the  Ackerman  steer  vehicle  is  presented  for  reference  purposes. 

This  report  is  both  a  tutorial  and  a  reference  for  the  transforms  used  in  the 
RANGER  vehicle  controller.  It  is  both  because  the  models  keep  evolving  and  it 
was  necessary  to  provide  the  tools,  mechanisms,  and  discipline  required  to  con¬ 
tinue  the  evolution. 
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1.  Introduction 


Kinematic  modelling  is  one  of  the  most  essential  analytical  tools  of  robotics.  It  is  used  for 
modelling  mechanisms,  actuators,  and  sensors  for  on-line  control,  off-line  programming,  and 
simulation  purposes.  This  document  presents  a  brief  survival  kit  of  concepts  and  techniques  that 
will  equip  the  reader  to  master  a  large  class  of  kinematic  modelling  problems. 

Control  of  autonomous  vehicles  in  3D  can  require  precise  kinematic  models  of  mechanisms,  the 
image  formation  process,  terrain  following,  steering  kinematics,  and  more.  The  document  provides 
the  tools  necessary  to  solve  these  problems  in  one  place  for  reference  purposes. 

JL1  Acknowledgments 

Learning  how  to  do  kinematics  has  been  a  long  process  that  started  with  my  bachelors  thesis  done 
under  Andrew  Goldenberg  at  the  University  of  Toronto  in  1984.  Sid  Skull  supervised  my  AI 
project  at  York  University  which  was  a  PROLOG  based  symbolic  mathematics  package  and  an 
expert  system  for  robot  kinematic  modelling.  At  CMU,  Martial  Hebert  and  Jay  Gowdy  started  my 
interest  in  the  peculiarities  of  modelling  the  kinematics  of  rangefinders.  Omead  Amidi  first  showed 
me  the  bicycle  model  for  Ackerman  vehicles.  Anthony  Stentz  first  referred  me  to  the  need  for 
projection  lookup  tables  for  fast  perception  for  off  road  purposes. 

LZ  Commentary 

Every  year,  I  have  to  learn  this  stuff  all  over  again,  so  I  finally  bit  the  bullet  and  tried  to  write  down 
the  issues  that  seem  important  to  me.  Somehow,  all  these  matrices  with  all  those  strings  of  trig 
functions  in  them  have  always  seemed  awkward  and  hard  to  figure  out.  Somewhere  in  the  past  there 
was  an  obscure  book  on  computer  graphics  that  presented  this  information  from  the  operator/ 
transform  duality  point  of  view  that  seemed  to  me  clear  and  concise.  I’ve  never  been  able  to  find  it 
again. 

This  report  is  both  a  tutorial  and  a  reference  for  the  transforms  used  in  the  RANGER  vehicle 
controller.  It  is  both  because  the  models  keep  evolving  and  it  was  necessary  to  provide  the  tools, 
mechanisms,  and  discipline  required  to  continue  the  evolution. 

1*2  Notational  Conventions 

The  3X3  matrix  denotes  a  rotation  matrix  which  transforms  a  displacement  from  its 
expression  in  coordinate  system  ‘a’  to  its  expression  in  coordinate  system  ‘b\  The  4X4  matrix 
T®  denotes  the  homogeneous  transform  which  transforms  a  vector  from  its  expression  in 
coordinate  system  ‘a’  to  its  expression  in  coordinate  system  ‘b’.  If  p  denotes  a  point  expressed 
in  coordinate  system  ‘a’,  then  the  notation  for  conversion  of  coordinates  to  coordinate  system  ‘b’ 
is  easy  to  remember  by  considering  that  the  a’s  cancel: 


The  4X4  matrix  denotes  a  nonlinear  projection  operator  represented  as  a  homogeneous 
transform.  In  such  notation,  the  vector  normalization  step  is  implicit  in  the  transform.  The  3X1 
vector  f  represents  the  translation  vector  from  system  ‘a’  to  system  ‘b’  expressed  in  system  ‘a’ 
coordinates.  The  matrix  denotes  the  Jacobian  of  the  transformation  from  system  ‘a’  to  system 

•b\ 
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The  specification  of  derivatives  will  be  necessarily  loose.  If  x  and  y  are  scalars,  x  and  y  are 
vectors,  and  X  and  Y  are  matrices,  then  all  of  the  following  derivatives  can  be  defined. 


dy 

dx 

a  partial  derivative 

dy 

a  gradient  vector 

dy 

5^ 

a  vector  partial  derivative 

dy 

51 

a  Jacobian  matrix 

dY 

dx 

a  matrix  partial  derivative 

9Y 

51 

a  Jacobian  tensor 

The  notation  ca  and  sa  is  shorthand  for  cos(a)  and  sin(a)  respectively.  Sums  of  angles,  for  example, 
will  be  indicated  by  cascaded  subscripts  as  follows: _ 

c  123  =  c<w(V1  +  \t'2  +  V3) 

Bolded  italic  text  is  used  for  emphasis,  whereas  bolded  nonitalic  text  highlights  key  words  that 
appear  in  the  index. 
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2.  Homogeneous  Coordinates  and  Transforms 

Homogeneous  coordinates  are  a  method  of  representing  3D  entities  by  the  projections  of  4D 
entities  onto  a  3D  subspace.  This  section  investigates  why  such  an  artificial  construct  has  become 
the  cornerstone  of  robot  kinematic  modelling. 

2.1  Points 

A  point  is  a  position  in  space.  Points  will  be  represented  by  column  vectors  such  as: 

T 

Pi  =  [*!  yj  zi] 

where  the  superscript  T  denotes  the  matrix  transpose  and  permits  writing  a  column  vector  as  a  row 
vector. 

12  Operators 

An  operator  is  any  process  which  maps  points  onto  other  points.  For  the  present  purpose, 
operators  will  be  limited  to  those  which  can  be  represented  as  matrices.  The  above  point  Pj  can 
be  altered  in  many  different  ways  by  multiplying  it  by  a  general  3X3  matrix  T : 


x2 

*xx  *xy  *xz 

X1 

Vi+Vi+,«zi 

P2  = 

y  2 

II 

H 

TJ 
►— * 

II 

tyX  tyy  tyZ 

*1 

ty*xl+tyyyl+VZl 

z2 

*zx  *zy  *zz 

Z1 

Vll+tzyyl+t«Zl. 

Some  simple  operators  are  scale,  reflection,  rotation,  projection,  and  shear.  All  of  these  can  be 
generated  from  the  above  matrix  by  a  suitable  choice  of  the  entries  in  the  matrix. 

12  Homogeneous  Coordinates 

Even  though  the  above  operator  is  very  general,  it  is  not  general  enough  to  be  used  conveniently 
in  many  robotics  applications  because  it  cannot  represent  a  translation.  That  is,  there  exists  no  3  X 
3  matrix  which  adds  a  constant  vector,  like  pg  to  p  j.  Such  a  translation  could  be  expressed  as  a 


This  cannot  be  done  by  a  matrix  because  p^  cannot  be  represented,  in  general,  as  a  linear 
combination  of  the  elements  of  p  ^  -  it  is  supposed  to  be  independent  of  p  j . 

However,  the  situation  can  be  fixed  with  a  standard  trick.  In  homogeneous  coordinates,  an  extra 
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element  can  be  added  to  each  point  to  represent  a  kind  of  scale  factor: 

T 

Pi  =  [xi  yi  zi  wi] 


and  it  is  conventional  to  consider  that  this  4-vector  is  projected  into  3D  by  dividing  by  the  scale 
factor: 


T 

Pi  = 

xi  yi  zi 

wi  wi  wi 

Points  are  typically  represented  with  a  scale  factor  of  1.  Thus: 

T 

pi  =  [xi  yi  zi !] 


is  a  point  in  homogeneous  coordinates.  It  is  also  possible  to  represent  a  pure  direction  in  terms  of 
a  point  at  infinity  by  using  a  scale  factor  of  0.  Thus: 

T 

_ qt  =  [*!  yt  OJ _ 

is  a  direction  in  homogeneous  coordinates. 


ZA  H9m<?g^nggiis_Tfr^p§f9nn^ 


Using  homogeneous  coordinates,  it  is  now  possible  to  represent  the  addition  of  two  vectors  as  a 
matrix  operation,  thus: 


p2  =  p,+p3  = 


X1 

mR 

xi 

yi 

+ 

ys 

yi 

zi 

WEE* 

zi 

i_ 

0 

_i. 

=  Trans  (p3)Pj 


where  Trans  (P3)  is  the  homogeneous  transform  which  is  equivalent  to  a  translation  operator 
for  the  translation  vector  pg. 
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15  Homogeneous  Transforms  as  Operators 

Suppose  it  is  necessary  to  move  a  point  in  some  manner  and  express  the  result  in  the  same 
coordinate  system  as  the  original  point.  This  is  the  notion  of  an  operator.  The  basic  operators  are 
translation  along  and  rotation  about  any  of  the  three  axes.  The  following  table  gives  the  four 
elementary  operators  which  are  sufficient  for  the  purposes  of  the  report,  and  for  almost  any  real 


It  is  important  to  remember  the  precise  semantics  of  these  operators.  They  take  a  point  expressed 
in  some  coordinate  system,  operate  on  it,  and  supply  the  result  expressed  in  the  same  coordinate 
system. 

One  of  the  most  common  operators  used  in  robotics  is  a  rotation  followed  by  a  translation.  The 
homogeneous  transform  can  be  used  to  represent  this  operator  as  a  single  matrix.  In  general,  many 
kinds  of  operators  can  be  represented  by  homogeneous  transforms.  However,  the  operator 
interpretation  is  but  one  of  three  possible  interpretations  of  the  homogeneous  transform. 
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IA  Example  -  Operating  on  a  Point 

The  homogeneous  coordinates  of  the  origin  o  are  clearly: 

T 

O  =  [o  0  0  l] 

As  an  example  of  applying  operators  to  a  point,  the  following  figure  indicates  the  result  of 
translating  the  origin  along  the  y  axis  by  V  units  and  then  rotating  the  resulting  point  by  90° 
around  the  x  axis. 


Notice  that  the  operators  are  written  in  right  to  left  order  because  this  is  the  order  in  which  they  are 
applied  to  the  original  column  vector  representing  the  origin.  The  order  is  important  because  matrix 
multiplication  is  not  commutative.  Also  notice  that  the  initial  point,  intermediate  point,  and  final 
result  are  all  expressed  in  terms  of  the  axes  of  the  original  coordinate  system.  Operators  have fixed 
axis  semantics. 

2J.  ftQmQgfflwiis-llramsforiffis  as  Coordinate?  raroes 

Recall  that  homogeneous  coordinates  can  be  used  to  represent  directions  as  well  as  points.  This  is 
done  by  using  a  zero  scale  factor.  The  unit  vectors  of  a  cartesian  coordinate  system  can  be 
considered  to  be  directions.  The  x,  y,  and  z  unit  vectors  can  be  written  as: _ 

T 

i  =  [l  0  0  0]  j  =  [0  1  0  0]T  k  =  [o  0  1  0]T 


Therefore,  these  three  columns  can  be  grouped  together  with  the  homogeneous  coordinates  of  the 
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origin  to  form  an  identity  matrix: 


100  0 
0  100 
00  10 
0001 


=  I 


Clearly,  the  columns  of  the  identity  matrix  can  be  interpreted  as  the  unit  vectors  and  the  origin  of 
a  coordinate  system.  The  result  of  applying  an  operator  to  these  four  vectors  has  a  similar 
interpretation  that  will  be  shown  by  example. 

%A  Example  -  Interpreting  an  Operator  as  a  Frame 

These  four  vectors  can  be  transformed  simultaneously  by  applying  the  same  two  operators  to  the 


This  result  is  just  the  product  of  the  original  two  operators.  Each  column  of  this  result  is  the 
transformation  of  the  corresponding  column  in  the  original  identity  matrix.  Thus  the  transformed 
vectors  are: 


It  has  been  shown  that  the  column  vectors  of  any  compound  fundamental  operator  are  just  the 
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homogeneous  coordinates  of  the  transformed  unit  vectors  and  the  origin.  This  is  because  the 
operators  apply  to  any  vectors  -  including  the  origin  and  the  unit  vectors.  The  result  can  be 
visualized  by  drawing  the  new  transformed  axes  at  the  new  position. 

2i2  The  Coordinate  Frame 

So  far  in  the  discussion,  there  are  two  complementary  interpretations  of  exactly  the  same  4X4 
matrix.  It  can  be  an  operator  which  moves  points  and  vectors  around,  or  it  can  be  a  representation 
of  a  cartesian  coordinate  system  positioned  somewhere  in  space  relative  to  another  one.  Cartesian 
coordinate  systems  positioned  in  space  are  a  central  concept  in  3D  kinematics.  They  encode  both 
a  position  and  an  attitude.  With  an  encoded  position  and  attitude  available,  it  becomes  possible  to 
talk  about  the  state  of  motion  of  the  origin  in  terms  of  translation  and  orientation  and  all  of  their 
associated  time  derivatives.  Therefore,  this  entity  embodies  the  properties  of  a frame  of  reference. 

With  a  set  of  three  orthogonal  unit  vectors,  it  is  possible  to  represent  an  arbitrary  vector  quantity  in 
terms  of  its  projections  onto  these  axes.  Therefore,  this  entity  also  embodies  the  properties  of  a 
cartesian  coordinate  system.  This  moving  set  of  unit  vectors  is  often  called  a  coordinate  frame  or 
simply  a  frame.  Often,  imaginary  frames  are  embedded  in  the  links  of  a  mechanism  in  order  to 
keep  track  of  its  configuration.  These  embedded  frames  are  central  to  the  study  of  manipulators, 
legs  and  other  mechanisms. 

2,10  Homogeneous  Transforms  as  Coordinate  Transforms 

There  is  a  third  interpretation  of  a  homogeneous  transform.  It  was  shown  that  they  move  points  and 
move  and  orient  frames  and  that  they  directly  represent  frames.  However,  if  they  move  frames,  one 
can  think  about  the  original  frame  and  the  transformed  frame  as  two  different  frames  and  then  ask 
about  the  relationship  between  the  coordinates  of  any  point  in  each  frame.  Let  the  original  frame 
be  called  ‘a’  and  the  transformed  one  be  called  *b\ 

Let  a  general  point  p  be  expressed  in  the  coordinates  of  frame  ‘b\  This  will  be  represented  by  a 
superscript  b.  Its  coordinates  in  this  frame  are: _ 

_ PM*Vzbl] _ 

Alternately,  it  can  be  written  as  a  true  physical  vector  thus: _ 


This  vector  can  be  expressed  in  the  coordinates  of  frame  ‘a’  by  expressing  the  unit  vectors  of  frame 
‘b’  in  the  coordinates  of  frame  ‘a’  and  adding  the  position  of  the  origin,  thus: _ 

pa  =  xbi'  +  ybj’  +  zbk’  +  o’ 
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It  has  been  shown  that  the  same  homogeneous  transform  that  moves  frame  ‘a’  into  coincidence 
with  frame  ‘b’  also  converts  the  coordinates  of  points  in  the  opposite  direction  -  from  frame  ‘b’ 
to  frame  ‘a’.  The  reason  for  the  opposite  sense  in  the  interpretation  is  that  moving  a  point  “forward” 
in  a  coordinate  system  is  completely  equivalent  to  moving  the  coordinate  system  “backward”. 


It  is  easy  to  show  that  the  inverse  of  a  rotation  matrix  is  equal  to  its  transpose.  This  is  called  the 
property  of  orthogonality.  This  leads  to  a  second  set  of  4  matrices,  the  fundamental  transforms, 
which  are  the  inverses  of  the  operators  and  which  therefore  convert  coordinates  from  ‘a’  to  ‘b\ 
These  are  given  in  the  table  below.  In  the  report,  the  transforms  will  be  denoted  with  lowercase 
case  first  letters  in  their  names. 
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i.n  example  -  lranstormmf  me  c. 


Again,  the  homogeneous  coordinates  of  the  origin  o  are: 

T 

o  =  [0  0  0  l] 


As  an  example  of  applying  transforms  to  a  point,  the  following  figure  indicates  the  result  of 
transforming  the  origin  of  frame  ‘b’  into  the  coordinates  of  frame  ‘a’  where  the  two  frames  are 
related  by  the  same  sequence  of  operators  used  earlier.  Frame  ‘b’  is  moved  into  coincidence  with 
frame  ‘a’  by  first  rotating  by  -90°  around  the  x  axis  of  frame  ‘b\  and  then  translating  by  -v  along 
the  z  axis  of  the  intermediate  frame. 


Notice  that  the  transforms  are  written  in  right  to  left  order  because  this  is  the  order  in  which  they 
are  applied  to  the  original  column  vector  representing  the  origin.  The  order  is  important  because 
matrix  multiplication  is  not  commutative.  Also  notice  that,  for  transforms,  each  new  transform  is 
interpreted  as  if  it  were  applied  to  the  last  frame  in  the  sequence  of  frames  that  ultimately  bring  the 
first  into  coincidence  with  the  last.  Transforms  have  moving  axis  semantics. 


Inxsrss  flLajjflmflgfinams  Transform 


The  homogeneous  transforms  that  will  be  used  in  this  report  will  all  be  structured  according  to  the 
following  template: 


The  scale  factor  will  almost  always  be  1  and  the  perspective  part  will  be  all  zeros  except  when 
modelling  cameras.  Under  these  conditions,  it  is  easy  to  show  by  multiplying  the  inverse  by  the 


Essential  Kinematics  for  Autonomous  Vehicles 


page  10. 


original  matrix,  that  the  inverse  is: 


This  is  very  useful  for  converting  from  a  matrix  that  converts  coordinates  in  one  direction  to  one 
that  converts  coordinates  in  the  opposite  direction.  Throughout  the  report,  remember  that  it  is 
trivial  to  reverse  the  sense  of  a  coordinate  transform. 

2.13  A  Duality  Theorem 

The  careful  reader  has  probably  noticed  that  operators  and  transforms  are  inverses.  Whether  a 
particular  matrix  is  considered  to  be  operator  or  transform  is  a  matter  of  taste.  This  has  an  important 
implication  for  kinematic  modelling  that  will  be  demonstrated  by  example.  Using  the  previous 
example,  the  moving  axis  operations  which  bring  frame  ‘a’  into  coincidence  with  frame  ‘b’  are: 

•  translate  v  units  along  the  z  axis 

•  rotate  90°  around  the  new  x  axis 


The  inverse  of  this  matrix  will  convert  coordinates  from  frame  ‘b’  to  frame  ‘a’,  and  as  was  shown 
earlier,  also  represents  the  position  and  attitude  of  frame  ‘b’  with  respect  to  frame  ‘a’.  Using  the 
well  known  method  for  inverting  a  matrix  product,  this  inverse  matrix  can  be  expressed  as  follows: 


TjJ  =  Trans  (0, 0, -v)-1Rotx(-7i/2)  1 
TjJ  =  Trans  (0, 0,  v)  Rotx  (rc/2) 

So  that  the  transform  from  ‘b’  to  ‘a’  can  be  written  in  terms  of  transforms  from  right  to  left  or  in 
terms  of  operators  in  the  reverse  order.  The  latter  view  is  traditional  in  robotics  and  it  will  be  used 
in  the  next  section.1 


1 .  One  implication  of  this  is  that  any  set  of  rotations  about  fixed  axes  is  equivalent  to  the  same  set  executed 
in  the  reverse  order  about  moving  axes. 
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3.  Forward  Kinematics 


Forward  kinematics  is  the  relatively  straightforward  process  of  chaining  homogeneous  transforms 
together  in  order  to  represent  the  articulations  of  a  mechanism  or  simply  to  represent  the  fixed 
transformation  between  two  frames.  In  this  process,  the  joint  variables  are  given,  and  the  problem 
is  to  find  the  transform. 

2*1  Nonlinear  Mapping 

It  is  clear  that  it  is  possible  to  transform  coordinates  between  coordinate  systems  which  are  fixed 
in  position  and  attitude  with  respect  to  each  other.  It  is  also  possible  to  perform  elementary 
operations  with  homogeneous  transforms.  Ultimately,  this  is  possible  because  such  operations  are 
linear.  That  is,  the  result  is  always  a  weighted  sum  of  the  original  point  or  vector. 

Most  mechanisms,  however,  are  not  linear.  They  are  composed  of  one  or  more  rotary  degrees  of 
freedom.  It  turns  out  that  the  homogeneous  transform  can  still  be  used  to  model  such  complex 
devices  if  they  are  viewed  in  a  different  way.  In  the  study  of  mechanisms,  the  paramet  r  the 
rotations  and  translations  between  the  frames  are  considered  to  be  variables.  In  this  .  the 
problem  is  one  of  understanding  the  variation  in  a  homogeneous  transform  when  it  is  considered 
to  be  a  function  of  one  or  more  variables. 

In  the  study  of  mechanisms,  the  real  world  space  in  which  a  mechanism  moves  is  often  called  task 
space,  and  points  and  vectors  are  normally  expressed  in  terms  of  cartesian  coordinates.  However, 
the  mechanism  articulations  are  most  easily  expressed  in  terms  of  angles  and  displacements  along 
axes.  This  space  is  called  configuration  space.  So  far,  we  have  studied  linear  transformations 
within  task  space.  This  section  and  die  following  sections  will  consider  the  more  difficult  problem 
of  expressing  the  relationship  between  task  space  and  configuration  space.  This  problem  is  one  of 
multidimensional  nonlinear  mapping. 

2*2  Mechanism  Models 

It  is  natural  to  think  about  the  operation  of  a  mechanism  in  a  moving  axes  sense  -  because  most 
mechanisms  are  built  that  way.  That  is,  the  position  and  orientation  of  any  link  in  a  kinematic  chain 
is  dependent  on  all  other  joints  that  come  before  it  in  the  sequence. 

Conventionally,  one  thinks  in  terms  of  moving  axis  operations  applied  to  frames  embedded  in  the 
mechanism  which  move  the  first  frame  sequentially  into  coincidence  with  all  of  the  others  in  the 
mechanism.  Then,  a  sequence  of  operators  is  written  to  represent  the  mechanism  kinematics.  The 
conventional  rules  for  modelling  a  sequence  of  connected  joints  are  as  follows: 

*  assign  embedded  frames  to  the  links  in  sequence  such  that  the  operations  which  move  each 
frame  into  coincidence  with  the  next  are  a  function  of  the  appropriate  joint  variable 

•  write  the  operator  matrices  which  correspond  to  these  operations  in  left  to  right  order 

This  process  will  generate  the  matrix  that  represents  the  position  of  the  last  embedded  frame  with 
respect  to  the  first,  or  equivalently,  which  converts  the  coordinates  of  a  point  from  the  last  to  the 
first.  This  matrix  will  be  called  the  mechanism  model. 
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felling  a  Masha 


M  M II 


In  1955,  J.  Denavit  and  R.  S.  Hartenberg  [3]  first  proposed  the  use  of  homogeneous  transforms  to 
represent  the  articulations  of  a  mechanism,  and  this  form  of  model  has  been  used  almost 
universally  since.  A  mechanism  is  considered  to  be  any  collection  of  joints,  either  linear  or  rotary, 
joined  together  by  links.  The  total  number  of  movable  joints  is  called  the  number  of  degrees  of 
freedom. 


It  is  conventional  in  many  aspects  of  robotics  to  use  a  special  product  of  four  fundamental  operators 
as  a  basic  conceptual  unit  The  resulting  single  matrix  can  represent  the  most  general  spatial 
relationship  between  two  frames.  This  convention  with  a  small  number  of  associated  rules  for 
assignment  of  embedded  frames  has  come  to  be  called  the  Denavit  Hartenberg  (DH)  convention. 


For  two  frames  positioned  in  space,  the  directions  of,  say,  the  z  axes  of  two  frames  define  two  lines 
in  3D  space.  For  nonparallel  axes,  there  is  a  well  defined  measure  of  the  distance  between  them 
given  by  the  length  of  their  mutual  perpendicular.  Let  the  first  frame  be  called  frame  i-1  and  the 
second  frame  i.  The  first  can  be  moved  into  coincidence  with  the  second  by  a  sequence  of  4 
operations: 

•  rotate  around  the  Xj.j  axis  by  an  angle  8, 

•  translate  along  the  Xj.j  axis  by  a  distance  Uj 

•  rotate  around  the  new  z  axis  by  an  angle  \|/j 

•  translate  along  the  new  z  axis  by  a  distance  w. 
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The  matrix,  conventionally  called  A*,  that  converts  coordinates  from  frame  i  to  frame  i-1  can  be 
written  using  the  rules  for  forward  kinematic  modelling  given  previously. 

AA  =  Tj-1  =  Rotx  (Op  Trans  (Uj,  0,0)  RotzCyp  Trans  (0,0,  wp 


Ai  =  T| 


10  0  0 

10  0  u- 

C\|T  — S\|/-  0  0 

\  00  0 

1  _ 

0  c0i  — s0j  0 

1 

0100 

s nr.  cijr  0  0 

010  0 

0  s0.  c0i  0 

001  0 

0  0  10 

00  1  WA 

0  0  0  1 

000  1 

0  0  0  1 

000  1. 

Aj  =  Tj-1 


ui 


— sxjfj  0 

cGjSXjr  cQic^i  -s0i  -sO^. 
sOjSijr  sOxijr  c0j  cGjW 
0  0  0  1 


This  matrix  has  the  following  interpretations: 


•  its  columns  represent  the  axes  and  origin  of  frame  i  expressed  in  frame  i-1  coordinates 

•  it  converts  coordinates  from  frame  i  to  frame  i- 1 
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2*5  Example  -  3  Link  Planar  Manipulator 

Although  the  homogeneous  transform  matrices  can  represent  any  complex  3D  mechanism,  the 
process  of  writing  forward  kinematic  equations  will  be  illustrated  for  a  simple  2D  manipulator 
composed  of  three  rotary  joints,  three  links  and  an  end  effector.  Coordinate  frames  are  assigned  to 
the  center  of  each  rotary  joint  and  the  datum  angle  for  each  is  set  to  zero  as  illustrated  in  the  figure 
below. 

It  is  conventional  to  assign  the  z  axis  of  a  frame  so  that  the  associated  degree  of  freedom,  linear  or 
rotary,  coincides  with  it.  Also,  the  x  axis  is  normally  chosen  so  that  it  points  along  the  mutual 


Table  1:  3  Link  Planar  Manipulator 


Link 

6 

u 

¥ 

w 

0 

0 

0 

Vi 

0 

1 

0 

Li 

0 

2 

0 

L2 

0 

3 

0 

L3 

0 

0 

This  gives  the  following  Denavit  Hartenberg  matrices.  Their  inverses  are  computed  from  the 
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inverse  formula  because  thev  will  be  useful  later: 


A  =  s 
A1  . 


1  C1 


0  0  10 
0  0  01 


Cj  Sj  00 

-Sj  Cj  0  0 

0  0  10 
0  0  01 


s2  c2  0  0 

0  0  10 
0  0  0  1 


C2  S2  0  c2L 

-s2  c2  0  s2Lj 

0  0  1  0 
0  0  0  1 


s3  c3  0  0 

0  0  10 
0  0  0  1 


C3  S3  0  c 3 L 

-S3  C3  0  s3L2 

0  0  1  0 

0  0  0  1 


010  0 
001  0 
000  1 


10  0  -Lr 

010  0 
001  0 
000  1 


The  position  and  orientation  of  the  end  effector  with  respect  to  the  base  is  given  by: 


_  rp0nplrr,2rj,3  _  A  A  A  A 
a4  “  a1a21314  ~  a1a2a3a4 


>t>0  _ 

A4  “ 


c j  Sj  0  0  c2  s2  0  Lj  C3  S3  0  L2  10  0  L3 

Sj  Cj  0  0  S2  C2  0  0  S3  C3  0  0  0100 

00100010  0010  001  0 
00010001  0001  Ipoo  1. 


rpO  _ 

1 4  - 


c123  s123  0  (c123L3  +  c12L2  +  c1LP 

s  123  c  123  ®  (s123^3  +  s12L2  +  s1LP 

0  0  1  0 

0  0  0  1 
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4.  Inverse  Kinematics 


Inverse  kinematics  is  the  problem  of  finding  the  joint  parameters  given  only  the  numerical  values 
of  the  homogeneous  transforms  which  model  the  mechanism.  In  practice,  this  is  useful  because  it 
solves  the  problem  of  where  to  drive  the  joints  in  order  to  get  the  hand  of  an  arm  or  the  foot  of  a 
leg  in  the  right  place,  at  the  right  orientation. 

Proficiency  with  inverse  kinematics  requires  a  degree  of  skill  and  practice  and  there  are  few  general 
guidelines  that  can  be  given.  This  section  will  discuss  this  difficult  problem  and  continue  with  the 
simple  example. 

4tl  Existence  and  Uniqueness 

The  inverse  kinematic  problem  is  considerably  more  difficult  than  the  forward  one  because  it 
involves  the  solution  of  nonlinear  equations.  In  general,  there  is  no  guarantee  that  nonlinear 
equations  have  a  solution,  and  if  they  do,  it  may  not  be  unique.  It  has  been  shown  theoretically  by 
Pieper  [13],  that  a  six  degree  of  freedom  manipulator  for  which  the  last  three  joints  intersect  at  a 
point  is  always  solvable.  Most  manipulators  are  constructed  in  this  way,  so  most  are  solvable. 

Any  real  mechanism  has  a  finite  reach,  so  it  can  only  achieve  positions  in  a  region  of  space  known 
as  the  workspace.  Unless  the  initial  transform  to  be  solved  is  in  the  workspace,  the  inverse 
kinematic  problem  will  have  no  solution.  This  manifests  itself,  for  example,  as  inverse  sines  and 
cosines  of  arguments  greater  than  one.  Further,  mechanisms  with  many  rotary  joints  will  often  have 
more  than  one  solution,  which  will  exhibit  various  forms  of  symmetry.  The  latter  case  is  known  as 
redundancy. 

4,2  TfChnigy? 

This  problem  would  be  very  difficult  to  solve  without  the  discipline  afforded  by  the  use  of 
homogeneous  transforms.  Using  the  DH  convention,  a  mechanism  can  be  “solved”  more  or  less 
one  joint  at  a  time  by  a  process  of  rewriting  the  forward  kinematics  equations  in  several  different 
ways. 

Any  DH  matrix  has  only  six  degrees  of  freedom.  The  rotation  matrix  part  is  constrained  to  be 
orthonormal.  That  is,  the  rows  or  columns  must  be  of  unit  length  and  mutually  perpendicular.  The 
bottom  row  is  constrained  to  contain  zeros  and  ones.  The  position  vector  contains  three 
independent  values. 

The  inverse  kinematic  problem  is  solved  by  rewriting  the  forward  transform  in  many  different 
ways  in  an  attempt  to  isolate  unknowns.  Although  there  are  only  6  independent  relationships,  they 
can  be  rewritten  in  many  different  ways. 

Using  a  3  degree  of  freedom  mechanism  for  example,  the  forward  kinematics  can  be  written  in  all 
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of  these  ways  by  premultiplying  or  postmultiplying  by  the  inverse  of  each  link  transform  in  turn 


T°  =  A1A2A3A4 

T°  =  AjA2A3A4 

A7‘t!J  =  a2a3a4 

T^A"1  =  AjA2A3 

=  a3a4 

tJJa^Aj1  =  AjA2 

A-1A-1A-1T°  —  A 
a3  a2  a1  1 4  ~  a4 

TX1A31A21  =  Aj 

A^A^A^A^tJ  =  I 

tX  Va2V  =  i 

These  are,  of  course,  redundant  specifications  of  the  same  set  of  equations,  so  they  contain  no  new 
information.  However,  they  often  generate  equations  which  are  easy  to  solve  because  the  DH 
convention  tends  to  isolate  each  joint  It  is  conventional  to  use  the  left  column  of  equations,  but  the 
right  column  is  easier  to  follow  in  our  case,  so  it  will  be  used. 

41  Example  -  3  Link  Planar  Manipulator 


The  process  is  started  by  assuming  that  the  forward  kinematic  solution  is  known.  Names  are 
assigned  to  each  of  its  elements  as  follows:  _ 


The  first  equation  is  already  known: 
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These  can  be  squared  and  added  to  yield: 


The  result  implies  that  there  are  two  solutions  for  this  angle  which  are  symmetric  about  zero.  These 
correspond  to  the  elbow  up  and  elbow  down  configurations.  Now,  before  the  expressions  were 


reduced  to  include  a  sum  of  angles,  they  were: 

kj  =  -riiL3  +px  =  Cj  (Cjl^  +  Lj)  -  Sj  (s2L2) 
k2  =  —  r21^"3"*"Py  =  (^2^2  "** ^1^  "**^i(s2L2) 


M  Standard  Emails 

There  are  a  few  forms  of  trigonometric  equations  that  recur  is  most  mechanisms.  The  following  set 
of  solutions  is  sufficient  for  most  applications.  One  of  the  most  difficult  forms,  solved  by  square 
and  add,  was  presented  in  the  example.  In  the  following,  the  letters  a,  b,  and  c  represent  arbitrary 
known  expressions. 


4.4.1  Explicit  Tangent 


This  form  generates  a  single  solution  because  both  the  sine  and  cosine  are  fixed  in  value.  It  can 
arise  in  the  last  two  joints  of  a  three  axis  wrist,  for  example.  The  equation: 


has  the  trivial  solution: 


\jf  =  atan2  (b,  a)  ( 

n  i 

j 

4.4.2  Point  Symmetric  Redu  ndancy 

This  form  generates  two  solutions  that  are  symmetric  about  the  origin.  It  can  arise  in  the  shoulder 
or  hip  joint,  for  example.  The  equation: _ 


can  be  solved  by  isolating  the  ratio  of  the  two  trig  functions.  There  are  two  angles  in  one  revolution 


\|/n  =  atan2  (b,  a) 

\|/n  =  atan2(-b,  -a) 


4.4.3  Line  Symmetric  Redundancy 

This  form  generates  two  solutions  that  are  symmetric  about  an  axis  because  the  sine  or  cosine  of 
the  deviation  from  the  axis  must  be  constant.  The  sine  case  will  be  illustrated.  It  can  arise  in  the 
shoulder  or  hip  joint,  or  in  an  elbow  or  knee  joint,  for  example.  The  equation: _ 

s  a  -  c  b  =  c 

n  n 

can  be  solved  by  the  trig  substitution: _ 

a  =  tcos(Q)  b  =  rsin  (0) 

where:  _ 

r  =  ±sqrt(a2  +  b2)  0  =  atan2(b,  a) 

This  gives: _ 

sc0  -  c  s0  =  c/r 

n  n 

s(0~  Vn)  =  c'/r 

So  the  cosine  is: _ 

c(0~Vn)  =  ±sqrt ( 1  -  (c/r)2) 

This  gives  the  result: 


Essential  Kinematics  for  Autonomous  Vehicles 


page  22. 


5.  Differential  Kinematics 


Differential  kinematics  is  the  study  of  the  derivatives  of  kinematic  models.  These  derivatives  are 
called  Jacobians  and  they  have  many  uses  ranging  from: 


•  resolved  rate  control 

•  sensitivity  analysis 

•  uncertainty  propagation 

•  static  force  transformation 


The  derivatives  of  the  fundamental  operators  with  respect  to  their  own  parameters  will  be 
important  They  can  be  used  to  compute  derivatives  of  very  complex  expressions  by  using  the 
chain  rule  of  differentiation.  For  reference,  they  are  quoted  below: 


Similar  expressions  can  be  generated  for  the  fundamental  transforms. 


If  a  sequence  of  joints  can  be  represented  by  the  product  of  a  series  of  homogeneous  transforms,  it 
is  natural  to  ask  about  the  effect  of  a  differential  change  in  joint  variables  on  the  position  and 
orientation  of  the  end  of  the  mechanism.  A  frame  matrix  represents  orientation  indirectly  in  terms 
of  three  unit  vectors,  so  an  extra  set  of  equations  is  required  to  extract  three  angles  from  the  rotation 
matrix  in  order  to  represent  orientation.  In  terms  of  position,  however,  the  last  column  of  the 
mechanism  model  gives  the  position  of  the  end  effector  with  respect  to  the  base. 


In  general,  let  a  mechanism  have  variables  represented  by  the  vector  q,_and  let  the  position  and 
orientation,  or  pose,  of  the  end  of  the  mechanism  be  given  by  the  vector  x .  Then,  the  end  effector 
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where  the  nonlinear  multidimensional  function  F  comes  from  the  mechanism  model.  The  Jacobian 
matrix  is  a  multidimensional  derivative  defined  as: 


j=S=l;(F®>  = 


The  differential  mapping  from  small  changes  in  q  to  the  corresponding  small  changes  in  x  is: 


dx  =  Jdq 


The  Jacobian  also  gives  velocity  relationships  via  the  chain  rule  of  differentiation  as  follows: 


dx  _  dx .  ,dq 

dt  "  5q  (dt 


which  maps  joint  rates  onto  end  effector  velocity.  Note  that  the  Jacobian  is  nonlinear  in  the  joint 
variables,  but  linear  in  the  joint  rates.  This  implies  that  reducing  the  joint  rates  by  half  reduces  the 
end  velocity  by  exactly  half  and  preserves  the  direction. 

Singularity 

Redundancy  takes  the  form  of  singularity  of  the  Jacobian  matrix  in  the  differential  kinematic 
solution.  A  mechanism  can  lose  one  or  more  degrees  of  freedom: 


•  at  points  where  two  different  inverse  kinematic  solutions  converge 

•  when  joint  axes  become  aligned  or  parallel 

•  when  the  boundaries  of  the  workspace  are  reached 

Singularity  implies  that  the  Jacobian  loses  rank  and  is  not  invertible.  At  the  same  time,  the  inverse 
kinematic  solution  tends  to  fail  because  axes  become  aligned,  and  infinite  rates  can  be  generated 
by  rate  control  laws. 
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For  the  example  manipulator,  the  last  column  of  the  manipulator  model  gives  the  following  two 
equations: 


which  can  be  differentiated  with  respect  to  Vj  >  ^2’  ^  V3  “  or<*er  t0  determine  the  velocity  of 
the  end  effector  as  the  joints  move.  The  solution  is: _ 

x  =  ~  (Sj23Vj23^3  +  s12^12^2  +  sl^l^l^ 
y  =  (c123V123L3  +  ci2Mfi2L2  +  cl'i/iL1) 


which  can  be  written  as: 


It  is  known  from  the  implicit  function  theorem  of  calculus  that  the  ratio  of  differential  volumes 
between  the  domain  and  range  of  a  multidimensional  mapping  is  given  by  the  Jacobian 
determinant.  This  quantity  has  applications  to  a  technique  of  navigation  and  ranging  called 

triangulation. 


Thus  the  product  of  the  differentials  forms  a  volume  in  both  configuration  space  and  in  task  space. 
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&£  Jacobian  Tensor 

At  times  it  is  convenient  to  compute  the  derivative  of  a  transform  matrix  with  respect  to  a  vector 
of  variables.  In  this  case,  the  result  is  the  derivative  of  a  matrix  with  respect  to  a  vector.  For 
example: 


This  is  a  three  dimensional  cube  of  numbers  which  can  loosely  be  called  a  tensor.  The  mechanism 
model  itself  is  a  matrix  function  of  a  vector. 


For  example,  if: 


Then  there  are  three  slices  of  the  tensor,  each  a  matrix,  given  by: 
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6.  Vehicle  Kinematics 


Some  aspects  of  the  kinematics  of  moving  vehicles  are  simpler  than  that  of  mechanisms,  others  are 
more  difficult  This  section  presents  an  abbreviated  discussion  of  the  kinematic  transforms 
necessary  for  control  of  a  vehicle  in  3D. 

£il  Axis  Conventions 

There  are  at  least  two  prevailing  conventions  for  the  assignment  of  axes  for  the  coordinate  systems 
of  vehicles.  In  aerospace  vehicles,  it  is  conventional  to  point  the  z  axis  downward,  and  this  makes 
it  natural  to  point  the  x  axis  forward  and  the  y  axis  out  the  right  side  of  the  vehicle.  The  convention 
used  here  is  that  the  z  axis  points  up,  y  forward,  and  x  out  the  right  side.  This  has  the  advantage 
that  the  projection  of  3D  information  onto  the  x-y  plane  is  more  natural. 

It  is  important  to  note  that  the  form  of  rotation  matrices  depends  firstly  on  the  order  of  their 
component  rotations,  and  secondly  on  the  linear  axis  conventions.  The  convention  used  here 
corresponds  to  a  z-x-y  Euler  angle  sequence.  Therefore,  it  is  not  advisable  to  use  the 
homogeneous  transforms  developed  here  until  they  are  verified  to  be  correct  for  any  sensors  and 
actuators  that  are  used. 


6*2  Frame  Assignment 

Several  coordinate  frames  are  important  for  moving  vehicles.  For  legged  vehicles,  the  frames 
embedded  in  the  legs  are  assigned  according  to  the  DH  convention  and  the  frames  for  the  rest  of 
the  system  are  analogous  to  those  for  wheeled  vehicles.  This  section  will  present  a  set  of  frames 
for  wheeled  vehicles  that  occur  in  most  applications. 
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6.2.1  The  Navigation  Frame 

This  is  the  coordinate  system  in  which  the  vehicle  position  and  attitude  is  ultimately  required. 
Usually,  this  frame  is  taken  as  locally  level  (i.e.  the  z  axis  is  perfectly  aligned  with  the  local  gravity 
vector,  not  the  local  terrain  tangent  plane).  The  z,  up,  or  azimuth  axis  is  aligned  with  the  gravity 
vector,  the  y,  or  north  axis  is  aligned  with  the  geographic  pole2 3,  and  the  x  axis  points  east  to 
complete  a  right-handed  system.  In  some  applications,  any  frame  that  is  fixed  on  the  earth  is 
satisfactory  whether  or  not  it  is  aligned  with  the  earth’s  fields.  This  frame  is  identified  by  the  letter 
n. 

6.2.2  The  Body  Frame 

The  body  frame  is  positioned  at  the  point  on  the  vehicle  body  which  is  most  convenient  and  is 
considered  to  be  fixed  in  attitude  with  respect  to  the  vehicle  body.  For  Ackerman  steer  vehicles,  the 
center  of  rear  axle  is  a  natural  place  for  this  frame.  In  some  applications,  the  best  estimate  of  the 
position  of  the  center  of  gravity  is  more  appropriate.  The  z  axis  points  up,  y  forward,  and  x  out  the 
right  side.  This  frame  is  identified  by  the  letter  b. 

6.2.3  The  Positioner  Frame 

This  frame  is  positioned  at  the  point  on  or  near  any  position  estimation  system  which  reports  its 
own  position.  If  the  system  generates  attitude  and  attitude  rates  only,  this  frame  is  not  required 
because  the  attitude  of  the  device  will  also  be  that  of  the  vehicle.  For  an  INS,  this  is  typically  the 
center  of  the  IMU  and  for  GPS  it  is  the  phase  center  of  the  antenna^.  Axes  directions  are  defined 
similarly  to  the  body  frame  directions.  There  is  a  different  positioner  frame  for  each  positioning 
device.  This  frame  is  identified  by  the  letter  p. 

6.2.4  The  Sensor  Head  Frame 

Sometimes,  environmental  perception  sensors  are  mounted  on  stabilized  platforms  or  on  pan/tilt 
mechanisms.  These  provide  isolation  of  the  sensor  attitude  from  that  of  the  vehicle  and/or  the 
ability  of  the  system  to  literally  point  its  “head”.  Axes  directions  are  defined  similarly  to  the  body 
frame  directions.  In  cases  where  the  rotary  axes  of  the  device  all  intersect  at  a  point,  this  frame  is 
positioned  at  the  common  point  of  intersection  of  these  axes.  Axes  directions  are  defined  similarly 
to  the  body  frame  directions. 

In  cases  where  the  environmental  perception  sensor  axes  are  not  aligned  with  respect  to  those  of 
the  vehicle  (for  example,  when  the  sensor  looks  downward),  or  in  cases  where  there  is  a 
misalignment  which  must  be  accounted  for,  a  rigid  sensor  head  can  be  defined  which  tilts  the  body 
axes  into  coincidence  with  those  of  the  sensor.  This  frame  is  identified  by  the  letter  h. 

6.2.5  The  Sensor  Frame 

This  frame  is  positioned  at  the  center  of  the  environmental  perception  sensor  with  axis  definitions 
similar  to  the  body  frame  when  the  sensor  points  forward.  For  video  cameras,  it  is  positioned  on 
the  optical  axis  at  the  image  plane  behind  the  lens.  For  stereo  systems,  it  is  positioned  either 
between  both  cameras  or  is  associated  with  the  image  plane  of  one  of  them.  For  imaging  laser 


2.  The  geographic  pole  is  determined  by  the  earth’s  spin  axis,  not  the  magnetic  field. 

3.  The  antenna  may  be  nowhere  near  the  GPS  receiver. 
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rangefinders,  it  is  positioned  as  the  average  point  of  convergence  of  the  rays  through  each  pixel. 
Axes  directions  are  defined  similarly  to  the  body  frame  directions.  There  is  a  different  sensor  frame 
for  each  sensor. 

6.2.6  The  Wheel  Frame 

This  frame  is  positioned  at  the  center  of  the  wheel,  on  the  axle.  At  times  it  may  be  convenient  to 
define  this  frame  at  the  wheel  contact  point  and  it  may  be  useful  to  have  it  rotate  with  the  wheel. 
Each  wheel  has  its  own  frame. 

The  RPY  Transform 

For  the  purpose  of  kinematic  modelling,  any  of  the  former  frames  can  be  considered  to  be 
embedded  in  a  rigid  body.  It  turns  out  that  the  attitude  of  a  rigid  body  can  be  expressed  in  many 
different  ways.  It  is  usually  most  convenient  to  express  vehicle  attitude  in  terms  of  three  special 
angles  called  roll,  pitch,  and  yaw.  Luckily,  most  pan/tilt  mechanisms  are  kinematically  formed 
from  a  yaw  rotation  followed  by  a  pitch  with  no  roll,  so  they  are  a  degenerate  form  of  a  more 
general  transform. 

These  considerations  imply  that  a  general  homogeneous  transform,  called  the  RPY  transform,  can 
be  formed  which  is  similar  in  principle  to  the  DH  matrix,  except  that  it  has  three  rotations,  and 
which  can  serve  to  transform  between  the  body  frame  and  all  others.  There  are  six  degrees  of 
freedom  involved,  three  translations  and  three  rotations,  and  each  can  be  either  a  parameter  or  a 
variable. 

Let  two  general  frames  be  defined  called  ‘a’  and  ‘b’  and  consider  the  moving  axis  operations  which 
transform  frame  ‘a’  into  coincidence  with  frame  ‘b’.  In  order,  these  are: 

•  translate  along  the  (x,y,z)  axes  of  frame  ‘a’  by  (u,v,w)  until  its  origin  coincides  with  that  of 
frame  ‘b’ 

•  rotate  about  the  new  z  axis  by  an  angle  \|f  called  yaw 

•  rotate  about  the  new  x  axis  by  an  angle  6  called  pitch 

•  rotate  about  the  new  y  axis  by  an  angle  <|>  called  roll 

Angles  are  measured  counterclockwise  positive  according  to  the  right  hand  rule.  These  operations 
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are  indicated  below  for  the  case  of  transforming  the  navigation  frame  into  the  body  frame. 


The  forward  kinematic  transform  that  represents  this  sequence  of  operations  is,  according  to  our 


rules  for  forward  kinematics: 


This  matrix  has  the  following  two  interpretations4: 


•  its  columns  represent  the  axes  and  origin  of  frame  ‘b’  expressed  in  frame  ‘a’  coordinates 

•  it  converts  coordinates  from  frame  ‘b’  to  frame  ‘a’ 


The  matrix  can  be  considered  to  be  the  conversion  from  a  pose  to  a  coordinate  frame. 


4.  This  transform  applies  to  the  Litton  INS  on  the  HMMWV,  the  Stagget  stable  platform,  and  the  ROS  pan/ 
tilt  head. 
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&A  Frame  Parameters  for  Wheeled  Vehicles 

Using  the  RPY  transform,  it  is  now  possible  to  specify  the  parameters  and  variables  which  permit 
coordinate  transformation  from  anywhere  on  a  vehicle  to  anywhere  else.  This  is  accomplished  by 
specifying  the  six  degrees  of  freedom  in  a  table.  In  the  table,  var  indicates  a  variable  and  fixed 
indicates  a  fixed  parameter: 


Table  2:  Frame  Parameters  for  Wheeled  Vehicles 


Transform 

u 

V 

w 

V 

e 

♦ 

xrr  . . . 

Lb 

var 

var 

var 

var 

var 

var 

*-pD 

fixed 

fixed 

fixed 

var 

var 

fixed 

rj^ll 

As 

fixed 

fixed 

fixed 

fixed 

fixed 

fixed 

rpb 

P 

fixed 

fixed 

fixed 

fixed 

fixed 

fixed 

nnD 

1 W 

fixed 

fixed 

fixed 

fixed 

var 

fixed 

&£  Inverse  Kinematics  for  the  RPY  Transform 

The  inverse  kinematic  solution  to  the  RPY  transform  has  at  least  two  uses: 

•  it  gives  the  angles  to  which  to  drive  a  sensor  head,  or  a  directional  antenna  given  the  direction 
cosines  of  the  goal  frame 

•  it  gives  the  attitude  of  the  vehicle  given  the  body  frame  axes,  which  often  correspond  to  the 
local  tangent  plane  to  the  terrain  over  which  it  moves 

This  solution  can  be  considered  to  be  the  procedure  for  extracting  a  pose  from  a  coordinate  frame. 
There  are  many  different  ways  to  get  the  solution  from  different  elements  of  the  RPY  transform. 
The  one  used  here  is  useful  for  modelling  terrain  following  of  a  vehicle.  Proceeding  as  for  a 
mechanism,  the  elements  of  the  transform  are  assumed  to  be  known: 
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The  premultiplication  set  of  equations  will  be  used.  The  first  equation  is: 


TjJ  =  Trans  (u,  v,  w)  Rotz  (\y)  Rotx  (8)  Roty  (<j>) 


rll  r12  r13  Px 

(c\jfc<|>  -  S\|fS0S<(>) 

-S\(fC0  (C\|/S<|>  +  S\|/S0C<|>) 

u 

r21  r22  r23  Py 

- 

(S\jfC<j>  +  C\|fS0S<|>) 

cyc0 

(S\|fS<|)-C\|fS0C<|>) 

V 

r31  r32  r33  Pz 

-C0S(j) 

s0 

C0C<{> 

w 

n 1 1 

0 

0 

0 

1 

The  translational  elements  are  trivial.  From  the  (1,2)  and  (2,2)  elements: 

=  atari!  (r22,  -r12) 


This  implies  that  yaw  can  be  determined  from  a  vector  which  is  known  to  be  aligned  with  the  body 
y  axis.  The  second  equation  is: 

[Trans  (u,  v,  w)  ]_1TjJ  =  Rotz  (\y)  Rotx  (0)  Roty  (<j>) 

(c\|fc<(>  —  s^|fs0s<|>)  -S\|fc0  (C\|/S<j>  +  S\|/S0C<j>)  0 
(s\yc<|>  +  crys0s<|>)  c^cO  (s\|/s<J>-c\|/s0c<)>)  0 
-C0S<|>  S0  C0C<j>  0 

0  0  0  0 


which  generates  nothing  new.  The  next  equation  is: 

[Rotz(\|/)]_1  [Trans  (u,  v,w)]-1Tj  =  Rotx  (0)  Roty  (<J>) 

(rHc¥  +  r21s¥)  (ri2c¥  +  r22s¥)  (ri3c¥  +  r23s¥)  0  [  0  S#  6 

:-rusv  +  r21cv)  (-r^sy  +  r^cy)  (-r13sy  +  r23cy)  0  _  s0s<J>  c0  -s0c<t>  0 
r31  r32  r33  0  -c0s<|>  s0  c0c<|>  0 

0  0  0  1  L  0  0  0  1 

From  the  (3,3)  and  (3,2)  elements: _ 

0  =  atari!  (t ~x \2SV +  t22c^ 


rllr12r13°l 
r21r22r23°  = 
r31  r32  r33  0 

0  0  0  lj 


Which  implies  that  pitch  can  also  be  determined  from  a  vector  known  to  be  aligned  with  the  body 
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y  axis.  A  good  solution  for  <jl  is  available  from  the  (1,1)  and  (13)  elements.  However,  for  reasons 
of  convenience,  the  solution  will  be  delayed  until  the  next  equation.  The  next  equation  is: 

[Rotx  (0)  ]  1  [Rotz  (\|f)  ] -1  [Trans  (u,  v,  w)  ] _1T^  =  Roty  (<)>) 

(rncV  +  r2isV)  (fi2cV  +  r22sV>  (r^cy  +  r^sy)  0 

c0[-rnsy  +  r21cy] +r31s0  c0  [-r12sy  +  r^cy]  +  r32s0  C0  [-r13sy +  r23cy] +r33s0  0 
-s0  [-rnsy +  r21cy]  +r31c0  -sOl-r^sy  +  r^cy]  +  r32c0  -s0[-r13sy +  r23cy]  +  r33c0  0 
L  0  0  0  1 


c<J>  0  s(j>  0 
0  10  0 
-s<j>  0  c<j>  0 
.  0  0  0  1 


From  the  (1,1)  and  (3,1)  elements:  _ 

(j)  =  atan2(sQ  [— rjjSXjf  +  r21c\|f]  -r31c0,  (rnc\|/  +  r21s\|0 ) 

This  implies  that  roll  can  be  derived  from  a  vector  known  to  be  aligned  with  the  body  x  axis. 

&£  Angular  Velocity 

The  roll,  pitch,  and  yaw  angles  are,  as  we  have  defined  them,  measured  about  moving  axes. 
Therefore,  they  are  a  sequence  of  Euler  angles,  specifically,  the  z-x-y  sequence5.  The  Euler  angle 
definition  of  vehicle  attitude  has  the  disadvantage  that  the  roll,  pitch,  and  yaw  angles  are  not  the 
quantities  that  are  actually  indicated  by  strapped  down  vehicle  mounted  sensors  such  as  gyros. 

The  relationship  between  the  rates  of  the  Euler  angles  and  the  angular  velocity  vector  is  nonlinear. 
The  angles  are  measured  neither  about  the  body  axes  nor  about  the  navigation  frame  axes.  It  is 
important  to  know  the  exact  relationship  between  the  two  because  it  provides  the  basis  for 
determining  vehicle  attitude  from  angular  rate  measurements. 

In  order  to  determine  the  angular  velocity,  consider  that  the  total  angular  velocity  is  the  sum  of 
three  components,  each  measured  about  one  of  the  intermediate  axes  in  the  chain  of  rotations  which 
bring  the  navigation  frame  into  coincidence  with  the  body  frame.  Using  the  fundamental 
transforms,  each  of  the  three  rotation  rates  are  transformed  into  the  body  frame  by  the  remaining 


5.  The  sequence  depends  on  the  convention  for  assigning  the  directions  of  the  linear  axes. 
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rotations  in  the  sequence  to  give  the  result  in  the  body  frame. 


This  result  gives  the  vehicle  angular  velocity  expressed  in  the  body  frame  in  terms  of  the  Euler 
angle  rates.  Notice  that  when  the  vehicle  is  level  the  x  and  y  components  are  zero  and  the  z 
component  is  just  the  yaw  rate  as  expected. 


Essential  Kinematics  for  Autonomous  Vehicles 


page  34. 


7.  Sensor  Kinematics 


Many  sensors  used  on  robot  vehicles  are  of  the  imaging  variety.  For  this  class  of  sensors,  the 
process  of  image  formation  must  be  modelled.  Image  coordinates  are  identified  by  the  letter  i. 
Typically,  these  transformations  are  not  linear,  and  hence  cannot  be  modelled  by  homogeneous 
transforms.  This  section  provides  the  transforms  necessary  for  modelling  such  sensors.  These 
transforms  include  projection,  reflection,  and  polar  coordinates. 

1A  Perspective  Projection 

In  the  case  of  passive  imaging  systems,  a  system  of  lenses  forms  an  image  on  an  array  of  sensitive 
elements  called  a  CCD.  These  systems  include  traditional  video  cameras  and  infra  red  cameras. 
The  transformation  from  the  sensor  frame  to  the  image  plane  row  and  column  coordinates  is  the 
standard  perspective  projection.  This  type  of  transform  is  unique  in  two  ways: 

» it  reduces  the  dimension  of  the  input  vector  by  one  and  hence  it  discards  information 

•  it  requires  a  post  normalization  step  where  the  output  is  divided  by  the  scale  factor  in  order  to 
re-establish  a  unity  scale  factor 

This  transformation  can  be  derived  by  similar  triangles. 


Note  in  particular  that  all  projections  are  not  invertible.  Here,  the  second  row  is  all  zeros,  so  the 
matrix  is  singular.  This,  of  course,  is  the  ultimate  source  of  the  difficulty  of  measuring  scene 
geometry  with  a  single  camera.  Therefore,  the  transform  is  identified  by  the  special  capital  letter  P. 
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•  the  incident  ray,  the  normal  to  the  surface,  and  the  reflected  ray,  all  lie  in  the  same  plane. 

•  the  angle  of  incidence  equals  the  angle  of  reflection 

From  these  two  rules,  we  can  formulate  a  very  useful  matrix  operator  to  reflect  a  vector  off  of  any 


This  can  be  reformulated  as  a  matrix  by  using  the  outer  product  ( <8> )  of  the  normal  with  itself: 
vr  =  Ref  (n)  Vj  Ref(n)  =  I-2(n®n) 

[l  -  n  n  n  n  n  n 

A  A  A  V  A  Z< 

Ref  (n)  =  nxny  l-nyny  nyiiz 

L  nxnz  nynz  1_nzn 

The  result  is  expressed  in  the  same  coordinates  in  which  both  the  normal  and  the  incident  ray  were 
expressed.  This  operator  can  be  used  to  model  the  “optical”  kinematics  of  laser  rangefinders. 
Notice  that  a  reflection  is  equivalent  to  a  rotation  of  twice  the  angle  of  incidence  about  an  axis 
which  is  normal  to  the  plane  formed  by  the  incident  ray  and  the  surface  normal.  A  similar  matrix 
refraction  operator  can  be  defined. 


Essential  Kinematics  for  Autonomous  Vehicles 


page  36. 


1A  Polar  Coordinates 

For  2D  scanning  laser  rangefinders,  the  transform  from  sensor  coordinates  to  image  coordinates  is 
the  spherical  polar  transform,  and  it  is  not  a  projection  because  such  a  sensor  generates  a 
measurement  of  range  as  well.  This  transform  is  a  nonlinear  function  that  cannot  be  modelled  as  a 
matrix. 


There  are  two  fundamental  possibilities  for  the  kinematics  of  such  sensors  because  there  are  two 
rotations  involved  which  may  occur  in  either  order.  These  are  derived  in  detail  in  [12].  These 
transforms  are  most  easily  derived  in  reverse.  That  is,  from  image  to  sensor  coordinates.  For  a 


sensor  w; 


here  the  azimuth  rotation  comes  first,  the  image  to  sensor  transform  is6: 


zs 

i 

Xs  =  Rs\|f 
ys  =  Rc0c\jf 
z„  =  -Rs0c\|/ 

*ys 

This  is  easily  inverted. 


x?  +  ys2  +  zs2 


atan  (x/Jy f+zf ) 
atari  (-z/ys) 


=  h  (x^,  yQ,  zj 


6.  This  transform  combined  with  the  last  two  provide  the  basics  of  a  computer  graphics  system  both  in  gen 
era 1,  and  for  its  particular  use  in  mobile  robot  simulation. 
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The  imaging  Jacobian  provides  the  relationship  between  the  differential  quantities  in  the  sensor 
frame  and  the  associated  position  change  in  the  image.  It  is  important  for  landmark  recognition 
applications. 


7.4.1  Perspective  Jacobian 
The  Jacobian  is: 


7.4.2  Azimuth  Polar  Jacobian 

When  the  azimuth  rotation  comes  first,  the  Jacobian  is: 
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L5  Projection  Tables 

The  directions  of  the  rays  through  each  pixel  of  an  imaging  sensor  are  fixed  with  respect  to  the 
sensor  frame.  The  trigonometric  overhead  of  computing  this  information  can  be  severe  in  some 
applications,  so  it  is  possible  and  worthwhile  to  compute  it  and  store  it  in  tables.  These  tables  can 
be  useful  both  in  real  time  perception  applications  and  in  off-line  ray  tracing  simulation.  This 
section  provides  the  equations  necessary  to  map  (row.column)  coordinates  onto  the  direction 
cosines  of  the  ray  through  those  coordinates. 

7.5.1  Perspective 

Let  the  width  of  the  image  plane  be  W  and  the  height  be  H  as  shown  in  the  figure.  Let  the 
horizontal  and  vertical  field  of  view  be  HFO  V  and  VFO  V. 


The  pers 

pective  transformation  is: 

X-  Xc  y. 

IS  ■'1 

7  '  T,  7  " 

ys  _ 
ys 

Z-  z 

1  T  =  - 

f  ys 

A  vector  from  the  sensor  frame  origin  to  the  image  plane  at  the  pixel  is: 

-A 

U  = 

x. f  zi 

T 
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A  unit  vector  in  this  direction  is: 


The  tangents  scale  linearly  with  distance  along  the  image  plane.  Therefore,  these  are  functions  of 


When  the  field  of  view  is  small  enough  to  satisfy  a  small  angle  assumption,  then  the  focus  distance 


and  this  permits  using  a  camera  model  that  is  independent  of  the  focal  length. 


7.5.2  Azimuth  Polar 

When  the  azimuth  rotation  of  a  scanning  mechanism  comes  first,  we  have  the  kinematic 
relationships: 


xs  =  Rs\|f 
ys  =  Rc0c\p 
z.  =  -Rs0c\|/ 

o 
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8.  Actuator  Kinematics 


For  wheeled  vehicles,  the  transformation  from  the  angles  of  the  steerable  wheels  and  their 
velocities  onto  path  curvatures,  or  equivalently  angular  velocities,  can  be  very  complicated.  One 
reason  for  this  is  that  there  can  be  more  degrees  of  freedom  of  steer  than  are  necessary.  In  this  case, 
the  equations  which  relate  curvature  to  steer  angle  are  overdetermined.  In  one  particular  case, 
however,  the  steering  mechanism  is  designed  such  that  this  will  not  be  the  case.  TTiis  mechanism 
is  used  on  most  conventional  automobiles  and  is  called  Ackerman  steering. 


It  is  useful  to  approximate  the  kinematics  of  the  Ackerman  steering  mechanism  by  assuming  that 
the  two  front  wheels  turn  slightly  differentially  so  that  the  instantaneous  center  of  rotation  of  the 
vehicle  can  be  determined  purely  by  kinematic  means.  This  amounts  to  assuming  that  the  steering 
mechanism  is  the  same  as  that  of  a  bicycle.  Let  the  angular  velocity  vector  directed  along  the  body 
z  axis  be  called  (3 . 


Using  the  bicycle  model  approximation,  the  path  curvature  K,  radius  of  curvature  R,  and  steer 
angle  a  are  related  by  the  wheelbase  L. 


Where  ta  denotes  the  tangent  of  a.  Rotation  rate  is  obtained  from  the  speed  V  as: 


Thus,  the  steer  angle  a  is  an  indirect  measurement  of  the  ratio  of  (3  to  velocity  through  the 


function: 


Although  it  is  common  to  think  of  these  equations  in  kinematic  terms,  this  is  only  possible  when 
the  dependence  on  time  is  avoided.  In  fact,  this  steering  mechanism  is  modelled  by  a  very 
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